
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_throw.c
  * @author     baiyang
  * @date       2023-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#if MODE_THROW_ENABLED == ENABLED
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mthrow_init(mode_base_t mode, bool ignore_checks);
static void mthrow_run(mode_base_t mode);
static ModeNumber mthrow_mode_number(mode_base_t mode);
static bool mthrow_requires_GPS(mode_base_t mode);
static bool mthrow_has_manual_throttle(mode_base_t mode);
static bool mthrow_allows_arming(mode_base_t mode, enum ArmingMethod method);
static bool mthrow_is_autopilot(mode_base_t mode);
static const char *mthrow_name(mode_base_t mode);
static const char *mthrow_name4(mode_base_t mode);
static bool throw_detected(ModeThrow* mode_throw);
static bool throw_attitude_good(ModeThrow* mode_throw);
static bool throw_height_good(ModeThrow* mode_throw);
static bool throw_position_good(ModeThrow* mode_throw);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_throw_ops = {
        .mode_number = mthrow_mode_number,
        .init        = mthrow_init,
        .exit        = NULL,
        .run         = mthrow_run,
        .requires_GPS = mthrow_requires_GPS,
        .has_manual_throttle = mthrow_has_manual_throttle,
        .allows_arming = mthrow_allows_arming,
        .is_autopilot = mthrow_is_autopilot,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mthrow_name,
        .name4 = mthrow_name4,
        .is_taking_off = NULL,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .set_speed_xy = NULL,
        .set_speed_up = NULL,
        .set_speed_down = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_throw_ctor(ModeThrow* mode_throw)
{
    mode_ctor(&mode_throw->mode, &mode_throw_ops);

    mode_throw->stage = Throw_Disarmed;
    mode_throw->prev_stage = Throw_Disarmed;
}

// init - initialise guided controller
static bool mthrow_init(mode_base_t mode, bool ignore_checks)
{
    ModeThrow* mode_throw = (ModeThrow*)mode;

    // do not enter the mode when already armed or when flying
    if (mb_motors_armed(mode->motors)) {
        return false;
    }

    // init state
    mode_throw->stage = Throw_Disarmed;
    mode_throw->nextmode_attempted = false;

    // initialise pos controller speed and acceleration
    posctrl_set_max_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), BRAKE_MODE_DECEL_RATE);
    posctrl_set_correction_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), BRAKE_MODE_DECEL_RATE);

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
    posctrl_set_correction_speed_accel_z(mode->pos_control, BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);

    return true;
}

// runs the throw to start controller
// should be called at 100hz or more
static void mthrow_run(mode_base_t mode)
{
    /* Throw State Machine
    Throw_Disarmed - motors are off
    Throw_Detecting -  motors are on and we are waiting for the throw
    Throw_Uprighting - the throw has been detected and the copter is being uprighted
    Throw_HgtStabilise - the copter is kept level and  height is stabilised about the target height
    Throw_PosHold - the copter is kept at a constant position and height
    */

    ModeThrow* mode_throw = (ModeThrow*)mode;

    if (!mb_motors_armed(mode->motors)) {
        // state machine entry is always from a disarmed state
        mode_throw->stage = Throw_Disarmed;

    } else if (mode_throw->stage == Throw_Disarmed && mb_motors_armed(mode->motors)) {
        gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
        mode_throw->stage = Throw_Detecting;

    } else if (mode_throw->stage == Throw_Detecting && throw_detected(mode_throw)){
        gcs_send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
        fms_set_land_complete(false);
        mode_throw->stage = Throw_Wait_Throttle_Unlimited;

        // Cancel the waiting for throw tone sequence
        notify_flags.waiting_for_throw = false;

    } else if (mode_throw->stage == Throw_Wait_Throttle_Unlimited &&
               mb_motors_get_spool_state(mode->motors) == MB_MOTOR_THROTTLE_UNLIMITED) {
        gcs_send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
        mode_throw->stage = Throw_Uprighting;
    } else if (mode_throw->stage == Throw_Uprighting && throw_attitude_good(mode_throw)) {
        gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
        mode_throw->stage = Throw_HgtStabilise;

        // initialise the z controller
        posctrl_init_z_controller_no_descent(mode->pos_control);

        // initialise the demanded height to 3m above the throw height
        // we want to rapidly clear surrounding obstacles
        if (fms.g.throw_type == Throw_Drop) {
            posctrl_set_pos_target_z_cm(mode->pos_control, ahrs_get_position_z_up_cm(mode->ahrs) - 100);
        } else {
            posctrl_set_pos_target_z_cm(mode->pos_control, ahrs_get_position_z_up_cm(mode->ahrs) + 300);
        }

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        fms_set_auto_armed(true);

    } else if (mode_throw->stage == Throw_HgtStabilise && throw_height_good(mode_throw)) {
        gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
        mode_throw->stage = Throw_PosHold;

        // initialise position controller
        posctrl_init_xy_controller(mode->pos_control);

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        fms_set_auto_armed(true);
    } else if (mode_throw->stage == Throw_PosHold && throw_position_good(mode_throw)) {
        if (!mode_throw->nextmode_attempted) {
            switch ((ModeNumber)fms.g.throw_nextmode) {
                case AUTO:
                case GUIDED:
                case RTL:
                case LAND:
                case BRAKE:
                case LOITER:
                    fms_set_mode((ModeNumber)fms.g.throw_nextmode, MODE_REASON_THROW_COMPLETE);
                    break;
                default:
                    // do nothing
                    break;
            }
            mode_throw->nextmode_attempted = true;
        }
    }

    // Throw State Processing
    switch (mode_throw->stage) {

    case Throw_Disarmed:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (fms.g.throw_motor_start == Throw_PMS_RUNNING) {
            mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_GROUND_IDLE);
        } else {
           mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_SHUT_DOWN);
        }

        // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
        attctrl_reset_yaw_target_and_rate(mode->attitude_control, true);
        attctrl_reset_rate_controller_I_terms(mode->attitude_control);
        attctrl_set_throttle_out(mode->attitude_control, 0, true, fms.g.throttle_filt);
        break;

    case Throw_Detecting:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (fms.g.throw_motor_start == Throw_PMS_RUNNING) {
            mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_GROUND_IDLE);
        } else {
            mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_SHUT_DOWN);
        }

        // Hold throttle at zero during the throw and continually reset the attitude controller
        attctrl_reset_yaw_target_and_rate(mode->attitude_control, true);
        attctrl_reset_rate_controller_I_terms(mode->attitude_control);
        attctrl_set_throttle_out(mode->attitude_control, 0, true, fms.g.throttle_filt);

        // Play the waiting for throw tone sequence to alert the user
        notify_flags.waiting_for_throw = true;

        break;

    case Throw_Wait_Throttle_Unlimited:

        // set motors to full range
        mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_THROTTLE_UNLIMITED);

        break;

    case Throw_Uprighting:

        // set motors to full range
        mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // demand a level roll/pitch attitude with zero yaw rate
        attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(mode->attitude_control, 0.0f, 0.0f, 0.0f);

        // output 50% throttle and turn off angle boost to maximise righting moment
        attctrl_set_throttle_out(mode->attitude_control, 0.5f, false, fms.g.throttle_filt);

        break;

    case Throw_HgtStabilise:

        // set motors to full range
        mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // call attitude controller
        attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(mode->attitude_control, 0.0f, 0.0f, 0.0f);

        // call height controller
        posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, 0.0f);
        posctrl_update_z_controller(mode->pos_control);

        break;

    case Throw_PosHold: {

        // set motors to full range
        mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // use position controller to stop
        Vector2f_t vel = VECTOR2F_DEFAULT_VALUE;
        Vector2f_t accel = VECTOR2F_DEFAULT_VALUE;
        posctrl_input_vel_accel_xy(mode->pos_control, &vel, &accel, true);
        posctrl_update_xy_controller(mode->pos_control);

        // call attitude controller
        Vector3f_t thrust_vector = posctrl_get_thrust_vector(mode->pos_control);
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, 0.0f, true);

        // call height controller
        posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, 0.0f);
        posctrl_update_z_controller(mode->pos_control);

        break;
        }
    }

#if 0
    // log at 10hz or if stage changes
    uint32_t now = AP_HAL::millis();
    if ((stage != prev_stage) || (now - last_log_ms) > 100) {
        prev_stage = stage;
        last_log_ms = now;
        const float velocity = inertial_nav.get_velocity_neu_cms().length();
        const float velocity_z = inertial_nav.get_velocity_z_up_cms();
        const float accel = copter.ins.get_accel().length();
        const float ef_accel_z = ahrs.get_accel_ef().z;
        const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
        const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
        const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
        const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
        
// @LoggerMessage: THRO
// @Description: Throw Mode messages
// @URL: https://ardupilot.org/copter/docs/throw-mode.html
// @Field: TimeUS: Time since system startup
// @Field: Stage: Current stage of the Throw Mode
// @Field: Vel: Magnitude of the velocity vector
// @Field: VelZ: Vertical Velocity
// @Field: Acc: Magnitude of the vector of the current acceleration
// @Field: AccEfZ: Vertical earth frame accelerometer value
// @Field: Throw: True if a throw has been detected since entering this mode
// @Field: AttOk: True if the vehicle is upright 
// @Field: HgtOk: True if the vehicle is within 50cm of the demanded height
// @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position
        
        AP::logger().WriteStreaming(
            "THRO",
            "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
            "s-nnoo----",
            "F-0000----",
            "QBffffbbbb",
            AP_HAL::micros64(),
            (uint8_t)stage,
            (double)velocity,
            (double)velocity_z,
            (double)accel,
            (double)ef_accel_z,
            throw_detect,
            attitude_ok,
            height_ok,
            pos_ok);
    }
#endif
}

static ModeNumber mthrow_mode_number(mode_base_t mode) { return THROW; }
static bool mthrow_requires_GPS(mode_base_t mode) { return true; }
static bool mthrow_has_manual_throttle(mode_base_t mode) { return false; }
static bool mthrow_allows_arming(mode_base_t mode, enum ArmingMethod method) { return true; };
static bool mthrow_is_autopilot(mode_base_t mode) { return false; }

static const char *mthrow_name(mode_base_t mode) { return "THROW"; }
static const char *mthrow_name4(mode_base_t mode) { return "THRW"; }

static bool throw_detected(ModeThrow* mode_throw)
{
    mode_base_t mode = &mode_throw->mode;

#if CONFIG_HAL_BOARD == HAL_BOARD_RTTHREAD
    uitc_estimator_status status;
    if (itc_copy_from_hub(ITC_ID(estimator_status), &status) == 0) {
        return false;
    }

    // Check that we have a valid navigation solution
    union ekf_solution_status ekf_solution_flags;
    ekf_solution_flags.value = status.solution_status_flags;
    if (!ekf_solution_flags.flags.attitude || !ekf_solution_flags.flags.pos_horiz_abs || !ekf_solution_flags.flags.pos_vert_abs) {
        return false;
    }
#endif

    // Check for high speed (>500 cm/s)
    bool high_speed = vec3_length_squared(ahrs_get_velocity_neu_cms(mode->ahrs)) > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);

    // check for upwards or downwards trajectory (airdrop) of 50cm/s
    bool changing_height;
    if (fms.g.throw_type == Throw_Drop) {
        changing_height = ahrs_get_velocity_z_up_cms(mode->ahrs) < -THROW_VERTICAL_SPEED;
    } else {
        changing_height = ahrs_get_velocity_z_up_cms(mode->ahrs) > THROW_VERTICAL_SPEED;
    }

    // Check the vertical acceleraton is greater than 0.25g
    bool free_falling = ahrs_get_accel_ef(mode->ahrs)->z > -0.25 * GRAVITY_MSS;

    // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
    bool no_throw_action = vec3_length(ahrs_get_accel(mode->ahrs)) < 1.0f * GRAVITY_MSS;

    // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
    bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;

    // Record time and vertical velocity when we detect the possible throw
    if (possible_throw_detected && ((time_millis() - mode_throw->free_fall_start_ms) > 500)) {
        mode_throw->free_fall_start_ms = time_millis();
        mode_throw->free_fall_start_velz = ahrs_get_velocity_z_up_cms(mode->ahrs);
    }

    // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
    bool throw_condition_confirmed = ((time_millis() - mode_throw->free_fall_start_ms < 500) && ((ahrs_get_velocity_z_up_cms(mode->ahrs) - mode_throw->free_fall_start_velz) < -250.0f));

    // start motors and enter the control mode if we are in continuous freefall
    return throw_condition_confirmed;
}

static bool throw_attitude_good(ModeThrow* mode_throw)
{
    // Check that we have uprighted the copter
    return (mode_throw->mode.ahrs->cos_roll * mode_throw->mode.ahrs->cos_pitch > 0.866f); // is_upright
}

static bool throw_height_good(ModeThrow* mode_throw)
{
    // Check that we are within 0.5m of the demanded height
    return (posctrl_get_pos_error_z_cm(mode_throw->mode.pos_control) < 50.0f);
}

static bool throw_position_good(ModeThrow* mode_throw)
{
    // check that our horizontal position error is within 50cm
    return (posctrl_get_pos_error_xy_cm(mode_throw->mode.pos_control) < 50.0f);
}
/*------------------------------------test------------------------------------*/
#endif

